/*=========================================================================
UberSim Source Code Release
-------------------------------------------------------------------------
Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
                   James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
                   Erick Tryzelaar {erickt}@andrew.cmu.edu
School of Computer Science, Carnegie Mellon University
-------------------------------------------------------------------------
This software is distributed under the GNU General Public License,
version 2.  If you do not have a copy of this licence, visit
www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
in the hope that it will be useful, but WITHOUT ANY WARRANTY,
including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-------------------------------------------------------------------------*/

#include <stdio.h>

#include "mmgr.h"

#include "BoundingSphere.h"
#include "Math.h"

/*******************************/

BoundingSphere::BoundingSphere ()
{
	radius = 0;
}

/*******************************/

BoundingSphere::BoundingSphere (const Vector3& center, const Real radius)
{
	this->center = center;
	this->radius = radius;
}

/*******************************/

BoundingSphere::BoundingSphere (const BoundingSphere& bound)
{
	this->center = bound.center;
	this->radius = bound.radius;
}

/*******************************/

BoundingSphere::~BoundingSphere ()
{
}

/*******************************/

Vector3& BoundingSphere::getCenter ()
{
	return center;
}

/*******************************/

Real& BoundingSphere::getRadius ()
{
	return radius;
}

/*******************************/

void BoundingSphere::setCenter (const Vector3& center)
{
	this->center = center;
}

/*******************************/

void BoundingSphere::setRadius (const Real radius)
{
	this->radius = radius;
}

/*******************************/

void BoundingSphere::operator += (const BoundingSphere& bound)
{
	Vector3 deltaCenter        = center - bound.center;
	Real    deltaRadius        = bound.radius - radius;
	Real    deltaRadiusSquared = deltaRadius * deltaRadius;
	Real    lengthSquared      = deltaCenter.lengthSquared ();

	if (deltaRadius > 0.0)
	{
		if (deltaRadiusSquared >= lengthSquared)
		{	
			center = bound.center;
			radius = bound.radius;
		}
		else
		{
			Real length = Math::sqrt (lengthSquared);
			Real alpha  = (length - deltaRadius) / (2.0 * length);
			center      = bound.center + alpha * deltaCenter;
			radius      = 0.5 * (bound.radius + length + radius);
		}
	}
	else if (deltaRadiusSquared < lengthSquared)
	{
		Real length = Math::sqrt (lengthSquared);
		Real alpha  = (length - deltaRadius) / (2.0 * length);
		center      = bound.center + alpha * deltaCenter;
		radius      = 0.5 * (bound.radius + length + radius);
	}
}

/*******************************/

bool BoundingSphere::isCollidingWith (BoundingSphere& bound)
{
	Vector3 c = center - bound.center;
	Real    r = radius + bound.radius;

/*
	printf ("me: xyz: %f %f %f r: %f\n", 
		center.x, 
		center.y, 
		center.z, 
		radius);
	printf ("him: xyz: %f %f %f r: %f\n", 
		bound.center.x, 
		bound.center.y, 
		bound.center.z, 
		bound.radius);

	printf ("diff: xyz: %f %f %f r: %f\n", 
		c.x, 
		c.y, 
		c.z, 
		r);

	printf ("diff: %f %f\n",
		c.lengthSquared (),
		r * r);
*/
	return (c.lengthSquared () <= r * r);
}

/*******************************/

BoundingSphere BoundingSphere::transformBy (Real scale, Matrix33& rotation, Vector3& translation)
{
	return BoundingSphere (translation + scale * (rotation * center), scale * radius);
}

/*******************************/
